﻿using Leadshine;
using System;
using System.Collections.Generic;
using System.IO;
using System.Text;
using System.Threading;
using System.Windows.Forms;
using System.Linq;
namespace MotionIoLib
{
    public class Motion_LTSMC : MotionCardBase
    {
        private ushort _ConnecNo = 0;

        public Motion_LTSMC(ulong indexCard, string strName, int nMinAxisNo, int nMaxAxisNo)
    : base(indexCard, strName, nMinAxisNo, nMaxAxisNo)
        {
            m_nCardIndex = indexCard;
        }

        public override bool AbsMove(int nAxisNo, double nPos, double nSpeed)
        {
            double pos = GetMotorType(nAxisNo) == MotorType.STEP ? GetAxisCmdPos(nAxisNo) : GetAxisActPos(nAxisNo);
            pos = nPos - pos;
            RelativeMove(nAxisNo, pos, nSpeed);
            return true;
        }

        public override bool AddAxisToGroup(int[] nAxisarr, ref object groupId)
        {
            return true;
        }
        List<object> list = new List<object>();
        public override bool SetBufMoveParam(object objGroup, ushort[] axisList, double velhigh, double vellow, double acc, double dec, ref string strError)
        {
            if (!list.Contains(objGroup))
                list.Add(objGroup);
            ushort CrdNo = (ushort)list.IndexOf(objGroup);
            if (velhigh == 0 || velhigh == 1 || velhigh == 2)
                velhigh += 0.01;
            if (vellow == 0 || vellow == 1 || vellow == 2)
                vellow += 0.01;
            List<double> list_VelHigh = new List<double>();
            List<double> list_VelLow = new List<double>();
            for (int i = 0; i < axisList.Length; i++)
            {
                double d_velHigh = velhigh;
                double d_velLow = vellow;
                TranMMToPluse(axisList[i], ref d_velHigh, ref acc, ref dec);
                TranMMToPluse(axisList[i], ref d_velLow, ref acc, ref dec);
                list_VelHigh.Add(d_velHigh);
                list_VelLow.Add(d_velLow);
            }
            velhigh = list_VelHigh.Min();
            vellow = list_VelLow.Min();
            //矩形插补运动非前瞻模式(dmc_conti_set_lookahead_mode)：前瞻模式矩形拐角会平滑处理变成圆弧，无法到达矩形端点；非前瞻模式则精确到达矩形端点
            int ret = LTSMC.smc_conti_set_lookahead_mode(_ConnecNo, CrdNo, 0, 1000, 100, 100000000);
            strError += $"lookahead_mode={ret}";
            ret = LTSMC.smc_conti_open_list(_ConnecNo, CrdNo, (ushort)axisList.Length, axisList);
            strError += $"open_list={ret}";
            ret = LTSMC.smc_set_vector_profile_unit(_ConnecNo, CrdNo, vellow, velhigh, acc, dec, vellow);
            strError += $"profile_unit={ret}";
            ret = LTSMC.smc_conti_set_blend(_ConnecNo, CrdNo, 1);
            return ret == 0; ;
        }

        public override bool AddBufDelay(object objGroup, int nTime, ref string strError)
        {
            if (nTime <= 0)
                return true;
            ushort CrdNo = (ushort)list.IndexOf(objGroup);
            double time = nTime / 1000.0;
            int ret = LTSMC.smc_conti_delay(_ConnecNo, CrdNo, time, 0);
            strError = ret.ToString();
            return true;
        }

        public override bool AddBufIo(object objGroup, string strIoName, bool bVal, ref string strError)
        {
            ushort CrdNo = (ushort)list.IndexOf(objGroup);

            var nIoIndex = IOMgr.GetInstace().GetOutputDic()[strIoName]._IoIndex;
            int ret = LTSMC.smc_conti_write_outbit(_ConnecNo, CrdNo, (ushort)nIoIndex, (ushort)(bVal ? 0 : 1), 0);
            strError = ret.ToString();
            return true;
        }

        public override bool AddBufMove(object objGroup, BufMotionType type, int mode, ushort[] axisList, double velHigh, double velLow, double[] Point1, double[] Point2, int Circle, ref string strError)
        {
            ushort CrdNo = (ushort)list.IndexOf(objGroup);

            ushort dirction = 0;
            if (type == BufMotionType.buf_Arc2dAbsCW)
            {
                dirction = 1;
            }
            else if (type == BufMotionType.buf_Arc2dAbsCCW)
            {
                dirction = 0;
            }
            var ret = 0;
            if (type == BufMotionType.buf_Arc2dAbsCCW || type == BufMotionType.buf_Arc2dAbsCW)
            {
                ret = LTSMC.smc_conti_arc_move_center_unit(_ConnecNo, CrdNo, (ushort)axisList.Length, axisList, Point2, Point1, dirction, Circle - 1, 1, 0);
                return ret == 0;
            }
            else if (type == BufMotionType.buf_Line2dAbs|| type == BufMotionType.buf_Line3dAbs)
            {
                ret = LTSMC.smc_conti_line_unit(_ConnecNo, CrdNo, (ushort)axisList.Length, axisList, Point2, 1, 0);
            }
            strError = ret.ToString();
            return ret == 0;
        }
        public override bool ClearBufMove(object objGroup)
        {
            ushort CrdNo = (ushort)list.IndexOf(objGroup);

            LTSMC.smc_conti_close_list(_ConnecNo, CrdNo);
            return true;
        }

        public override bool Close()
        {
            LTSMC.smc_board_close(_ConnecNo);
            return true;
        }

        public override int GetAxisActPos(int nAxisNo)
        {
            double pos = 0;
            if (GetMotorType(nAxisNo) == MotorType.STEP)
                LTSMC.smc_get_target_position_unit(_ConnecNo, (ushort)nAxisNo, ref pos);
            else
                LTSMC.smc_get_position_unit(_ConnecNo, (ushort)nAxisNo, ref pos);

            return (int)pos;
        }

        public override int GetAxisCmdPos(int nAxisNo)
        {
            double pos = 0;
            var ret = LTSMC.smc_get_target_position_unit(_ConnecNo, (ushort)nAxisNo, ref pos);
            return (int)pos;
        }

        public override int GetAxisPos(int nAxisNo)
        {
            double pos = 0;
            LTSMC.smc_get_position_unit(_ConnecNo, (ushort)nAxisNo, ref pos);
            return (int)pos;
        }

        public override long GetMotionIoState(int nAxisNo)
        {
            long lStandardIo = 0;
            uint status = LTSMC.smc_axis_io_status(_ConnecNo, (ushort)nAxisNo);
            bool ALM = ((int)status & 1) == 1;
            bool ELP = ((int)status & 2) == 2;
            bool ELN = ((int)status & 4) == 4;
            bool EMG = ((int)status & 8) == 8;
            bool ORG = ((int)status & 16) == 16;
            bool SLP = ((int)status & 64) == 64;
            bool SLN = ((int)status & 128) == 128;
            bool INP = ((int)status & 256) == 256;
            bool EZ = ((int)status & 512) == 512;
            bool RDY = ((int)status & 1024) == 1024;
            bool DSTP = ((int)status & 2048) == 2048;


            //long lStandardIo1 = 0;

            if (ALM)
            {
                lStandardIo |= (0x01 << 0);

            }
            else if (ELP)
            {
                lStandardIo |= (0x01 << 1);
            }
            else if (ELN)
            {
                lStandardIo |= (0x01 << 2);
            }
            else if (EMG)
            {
                lStandardIo |= (0x01 << 4);

            }
            else if (ORG)
            {
                lStandardIo |= (0x01 << 3);
            }


            if (GetServoState(nAxisNo))
            {
                lStandardIo |= (0x01 << 7);
            }
            return lStandardIo;
        }

        public override bool GetServoState(int nAxisNo)
        {
            var ret = LTSMC.smc_read_sevon_pin(_ConnecNo, (ushort)nAxisNo);
            return ret == 0;
        }

        public override bool Home(int nAxisNo, int nParam)
        {
            //LTSMC.smc_set_pulse_outmode(_ConnecNo, (ushort)nAxisNo, 0);//设置脉冲模式
            //LTSMC.smc_set_home_pin_logic(_ConnecNo, (ushort)nAxisNo, 0, 0);//设置原点低电平有效
            LTSMC.smc_set_home_profile_unit(_ConnecNo, (ushort)nAxisNo, 
                m_HomePrm[nAxisNo].VelL * (m_MovePrm[nAxisNo].PlusePerRote / m_MovePrm[nAxisNo].AxisLeadRange),
                m_HomePrm[nAxisNo].VelH * (m_MovePrm[nAxisNo].PlusePerRote / m_MovePrm[nAxisNo].AxisLeadRange), m_MovePrm[nAxisNo].AccL, m_MovePrm[nAxisNo].DccL);//设置起始速度、运行速度、停止速度、加速时间、减速时间
            LTSMC.smc_set_homemode(_ConnecNo, (ushort)nAxisNo, m_HomePrm[nAxisNo]._bHomeDir ? (ushort)1 : (ushort)0, 1, (ushort)m_HomePrm[nAxisNo]._nHomeMode, 0);//设置回零模式
            LTSMC.smc_set_home_position_unit(_ConnecNo, (ushort)nAxisNo, 2, m_HomePrm[nAxisNo]._iSeachOffstPluse);//设置偏移模式
            LTSMC.smc_home_move(_ConnecNo, (ushort)nAxisNo);//启动回零
            return true;
        }

        public override AxisState IsAxisNormalStop(int nAxisNo)
        {
            uint status = LTSMC.smc_axis_io_status(_ConnecNo, (ushort)nAxisNo);
            if (((int)status & 1) == 1)
                return AxisState.DriveAlarm;
            if (((int)status & 2) == 2)
                return AxisState.LimtPStop;
            if (((int)status & 4) == 4)
                return AxisState.LimtNStop;
            if (!GetServoState(nAxisNo))
                return AxisState.ServoOff;

            return IsInpos(nAxisNo) ? AxisState.NormalStop : AxisState.Homeing;
        }

        public override AxisState IsHomeNormalStop(int nAxisNo)
        {
            uint status = LTSMC.smc_axis_io_status(_ConnecNo, (ushort)nAxisNo);
            if (((int)status & 1) == 1)
                return AxisState.DriveAlarm;
            if (((int)status & 2) == 2)
                return AxisState.LimtPStop;
            if (((int)status & 4) == 4)
                return AxisState.LimtNStop;
            if (!GetServoState(nAxisNo))
                return AxisState.ServoOff;

            return IsInpos(nAxisNo) ? AxisState.NormalStop : AxisState.Homeing  ;
        }

        public override bool IsInpos(int nAxisNo)
        {
            return LTSMC.smc_check_done(_ConnecNo, (ushort)nAxisNo) == 1;
        }

        public override bool IsOpen()
        {
            return m_bOpen;
        }

        public override bool isOrgTrig(int nAxisNo)
        {
            uint status = LTDMC.dmc_axis_io_status(_ConnecNo, (ushort)nAxisNo);
            bool ORG = ((int)status & 16) == 16;
            return ORG;
        }

        public override bool JogMove(int nAxisNo, bool bPositive, int bStart, double nSpeed)
        {
            short ret = 0;
            double speed = nSpeed;
            double Acc = m_MovePrm[nAxisNo].AccH;
            double Dcc = m_MovePrm[nAxisNo].DccH;

            TranMMToPluse(nAxisNo, ref speed, ref Acc, ref Dcc);
            ret = LTSMC.smc_set_profile_unit(_ConnecNo, (ushort)nAxisNo, 0, speed, Acc, Dcc, 0);//设置起始速度、运行速度、停止速度、加速时间、减速时间
            LTSMC.smc_set_dec_stop_time(_ConnecNo, (ushort)nAxisNo, m_MovePrm[nAxisNo].DccL);//设置减速停止时间
            LTSMC.smc_vmove(_ConnecNo, (ushort)nAxisNo, (ushort)(bPositive ? 1 : 0));//连续运动
            return true;
        }

        public override bool Open()
        {
            _ConnecNo = (ushort)m_nCardIndex;
            for (int i = 0; i < 3; i++)
            {
                short res = LTSMC.smc_board_init(_ConnecNo, 2, $"192.168.5.{m_nCardIndex}", 115200);//连接控制器
                if (res != 0 && i > 1)
                {
                    return false;
                }
                if (res == 0) { break; }
                if (res != 0 && i == 0) { Close(); }
                Thread.Sleep(1000);
            }

            string str = System.AppDomain.CurrentDomain.BaseDirectory;
            string strConfigPath = "Motion_" + m_nCardIndex + ".cfg";
            strConfigPath = str + strConfigPath;

            string ext = Path.GetExtension(strConfigPath);
            ushort type = 0;
            if (string.Compare(ext, ".bas", true) == 0)
            {
                type = 0;
            }
            else if (string.Compare(ext, ".g", true) == 0 || string.Compare(ext, ".nc", true) == 0)
            {
                type = 1;
            }
            else if (string.Compare(ext, ".cfg", true) == 0)
            {
                type = 2;
            }
            FileStream fs = File.Open(strConfigPath, FileMode.Open, FileAccess.Read, FileShare.ReadWrite);
            StreamReader sr = new StreamReader(fs);
            str = sr.ReadToEnd();
            sr.Close();
            fs.Close();
            byte[] buffer = Encoding.UTF8.GetBytes(str);
            byte[] name = Encoding.UTF8.GetBytes(Path.GetFileName(strConfigPath));
            LTSMC.smc_download_memfile(_ConnecNo, buffer, (uint)buffer.Length, name, type);
            m_bOpen = true;
            return true;
        }

        public override bool ReasetAxis(int nAxisNo)
        {
            LTSMC.smc_write_erc_pin(_ConnecNo, (ushort)nAxisNo, (ushort)0);
            return true;
        }

        public override bool RelativeMove(int nAxisNo, double nPos, double nSpeed)
        {
            double speed = nSpeed;
            double Acc = m_MovePrm[nAxisNo].AccH;
            double Dcc = m_MovePrm[nAxisNo].DccH;

            TranMMToPluse(nAxisNo, ref speed, ref Acc, ref Dcc);
            double d_Radio = GetMotorType(nAxisNo) == MotorType.SEVER ? MotionMgr.GetInstace().d_Radio : 1;

            //高速起效
            if (nSpeed==0)
               speed = (speed * d_Radio);
            short ret = 0;
            ret = LTSMC.smc_set_profile_unit(_ConnecNo, (ushort)nAxisNo, 0, speed, Acc, Dcc, 0);//设置起始速度、运行速度、停止速度、加速时间、减速时间
            ret = LTSMC.smc_set_s_profile(_ConnecNo, (ushort)nAxisNo, 0, 0.1);
            if (ret != 0)
            {
                return false;
            }
            ret = LTSMC.smc_set_dec_stop_time(_ConnecNo, (ushort)nAxisNo, 0.1);//设置减速停止时间
            if (ret != 0)
            {
                return false;
            }
            LTSMC.smc_pmove_unit(_ConnecNo, (ushort)nAxisNo, nPos, 0);//连续运动
            return true;
        }

        public override bool ServoOff(short nAxisNo)
        {
            LTSMC.smc_write_sevon_pin(_ConnecNo, (ushort)nAxisNo, (ushort)1);
            return true;
        }

        public override bool ServoOn(short nAxisNo)
        {
            LTSMC.smc_write_sevon_pin(_ConnecNo, (ushort)nAxisNo, (ushort)0);
            return true;
        }

        public override bool SetActutalPos(int nAxisNo, double pos)
        {
            LTSMC.smc_set_encoder_unit(_ConnecNo, (ushort)nAxisNo, 0);//位置清零
            return true;
        }

        public override bool SetCmdPos(int nAxisNo, double Pos)
        {
            LTSMC.smc_set_position_unit(_ConnecNo, (ushort)nAxisNo, 0);//位置清零
            return true;
        }

        public override bool StartBufMove(object objGroup)
        {
            ushort CrdNo = (ushort)list.IndexOf(objGroup);
            int ret = LTSMC.smc_conti_start_list(_ConnecNo, CrdNo);
                 ret = LTSMC.smc_conti_close_list(_ConnecNo, CrdNo);
            return true;
        }

        public override bool StopAxis(int nAxisNo)
        {
            LTSMC.smc_stop(_ConnecNo, (ushort)nAxisNo, 0);
            return true;
        }
        public override bool TranMMToPluse(int nAxisNo, ref double dSpeed, ref double acc, ref double dec)
        {
            double speed = dSpeed;
            double Acc = m_MovePrm[nAxisNo].AccH;
            double Dcc = m_MovePrm[nAxisNo].DccH;
            switch (dSpeed)
            {
                case (int)SpeedType.High:
                    Acc = m_MovePrm[nAxisNo].AccH;
                    Dcc = m_MovePrm[nAxisNo].DccH;
                    speed = m_MovePrm[nAxisNo].VelH;
                    break;
                case (int)SpeedType.Low:
                    Acc = m_MovePrm[nAxisNo].AccL;
                    Dcc = m_MovePrm[nAxisNo].DccL;
                    speed = m_MovePrm[nAxisNo].VelL;
                    break;
            }
            //新添转换
            dSpeed = speed = (1 * speed / m_MovePrm[nAxisNo].AxisLeadRange) * m_MovePrm[nAxisNo].PlusePerRote / 1;//- 1- 1
            return true;
        }

        public override bool TransMMToPluseForHomeParam(int nAxisNo, ref double dVelH, ref double dVelL, ref double dAccH, ref double dAccL, ref double dDecH, ref double dDecL)
        {
            dVelH = (1 * m_HomePrm[nAxisNo].VelH / m_MovePrm[nAxisNo].AxisLeadRange) * m_MovePrm[nAxisNo].PlusePerRote / 1;

            dVelL = (1 * m_HomePrm[nAxisNo].VelL / m_MovePrm[nAxisNo].AxisLeadRange) * m_MovePrm[nAxisNo].PlusePerRote / 1;


            dAccH = (1 * m_HomePrm[nAxisNo].AccH / m_MovePrm[nAxisNo].AxisLeadRange) * m_MovePrm[nAxisNo].PlusePerRote / 1;
            dAccL = (1 * m_HomePrm[nAxisNo].AccL / m_MovePrm[nAxisNo].AxisLeadRange) * m_MovePrm[nAxisNo].PlusePerRote / 1;

            dDecH = (1 * m_HomePrm[nAxisNo].DccH / m_MovePrm[nAxisNo].AxisLeadRange) * m_MovePrm[nAxisNo].PlusePerRote / 1;
            dDecL = (1 * m_HomePrm[nAxisNo].DccL / m_MovePrm[nAxisNo].AxisLeadRange) * m_MovePrm[nAxisNo].PlusePerRote / 1;

            return true;
        }

        public override bool StopEmg(int nAxisNo)
        {
            LTSMC.smc_stop(_ConnecNo, (ushort)nAxisNo, 1);
            return true;
        }
        public override GpState GetGpState(object group)
        {
            return GpState.GpReady;
        }
    }
}